package org.example.util;

import org.example.bean.*;

import java.util.*;

/**
 * @Desc:
 * @Author: yanghongjun
 * @Date: 2024/5/14
 **/
public class ThetaStarHashUtil {


    private static final double DIAG_COST = 1.41421356237; // sqrt(2), 代表对角线移动的成本

    /**
     * 使用Theta*算法找到从起点到终点的路径。
     *
     * @param startNode 起点
     * @param goalNode  终点
     * @param gameMap   地图数据
     * @return 从起点到终点的路径，若无法到达则返回null
     */
    public static List<JsonGamePos> findPath(LazyNode startNode, LazyNode goalNode, GameHashMap gameMap) {
        System.out.println("Finding path...");
        long now = System.currentTimeMillis();
        PriorityQueue<LazyNode> openSet = new PriorityQueue<>(Comparator.comparingDouble(LazyNode::getfScore));
        HashSet<LazyNode> closedSet = new HashSet<>();
        List<JsonGamePos> jsonGamePos = new ArrayList<>();
        startNode.setgScore(0);
        startNode.sethScore(heuristic(startNode, goalNode));
        startNode.setfScore(startNode.getgScore() + startNode.gethScore());
        openSet.add(startNode);

        while (!openSet.isEmpty()) {
            LazyNode currentNode = openSet.poll();
            currentNode.setInOpenSet(false);

            if (currentNode.equals(goalNode)) {
                List<Cord> lazyNodes = reconstructPath(currentNode);
                System.out.println("ThetaStarHashUtil 寻路耗时 " + (System.currentTimeMillis() - now));
                long l = System.currentTimeMillis();
                jsonGamePos = gameMap.optimizePath(lazyNodes);
                System.out.println("ThetaStarHashUtil 平滑路径耗时：" + (System.currentTimeMillis() - l) + " 平滑前节点：" + lazyNodes.size() +
                        "平滑后节点：" + jsonGamePos.size());
                break;
            }

            closedSet.add(currentNode);

            for (LazyNode neighbor : gameMap.getHashNeighbors(currentNode)) {
                if (closedSet.contains(neighbor)) {
                    continue;
                }

                double tentativeGScore = currentNode.getgScore() + distBetween(currentNode, neighbor);

                if (!neighbor.isInOpenSet() || tentativeGScore < neighbor.getgScore()) {
                    neighbor.setParent(currentNode);
                    neighbor.setgScore(tentativeGScore);
                    neighbor.sethScore(heuristic(neighbor, goalNode));
                    neighbor.setfScore(neighbor.getgScore() + neighbor.gethScore());
                    openSet.add(neighbor);
                    neighbor.setInOpenSet(true);
                }
            }
        }
        for (LazyNode lazyNode : closedSet) {
            gameMap.getLazyNodePool().release(lazyNode);
        }
        return jsonGamePos;
    }

    /**
     * 根据给定的当前节点，重建并返回从起点到该节点的路径。
     *
     * @param currentNode 当前节点
     * @return 从起点到当前节点的路径
     */
    private static List<Cord> reconstructPath(LazyNode currentNode) {
        List<Cord> path = new ArrayList<>();
        int depth = 0;
        while (currentNode != null) {
            if (depth == 0 || depth != currentNode.getDirection() || currentNode.getParent() == null) {
                path.add(new Cord(currentNode.getX(), currentNode.getY()));
                depth = currentNode.getDirection();
            }
            currentNode = currentNode.getParent();
        }
        Collections.reverse(path);
        return path;
    }


    /**
     * 计算两个节点之间的启发式距离。
     *
     * @param nodeA 节点A
     * @param nodeB 节点B
     * @return 启发式距离
     */
    private static double heuristic(LazyNode nodeA, LazyNode nodeB) {
        return Math.abs(nodeA.getX() - nodeB.getX()) + Math.abs(nodeA.getY() - nodeB.getY());
    }

    /**
     * 计算两个节点之间的距离。
     *
     * @param nodeA 节点A
     * @param nodeB 节点B
     * @return 两点之间的距离
     */
    private static double distBetween(LazyNode nodeA, LazyNode nodeB) {
        double dx = nodeA.getX() - nodeB.getX();
        double dy = nodeA.getY() - nodeB.getY();

        if (dx == 0 || dy == 0) {
            return 1; // Cardinal move
        } else {
            return DIAG_COST; // Diagonal move
        }
    }


}
